/*
 * SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <pid_ctrl.h>
#include <stdbool.h>
#include <sys/param.h>
#include <stdio.h>
#include <stdlib.h>
static float pid_calc_positional(pid_ctrl_block_t *pid, float error) {
	float output = 0;
	/* Add current error to the integral error */
	pid->integral_err += error;
	/* If the integral error is out of the range, it will be limited */
	pid->integral_err = MIN(pid->integral_err, pid->max_integral);
	pid->integral_err = MAX(pid->integral_err, pid->min_integral);

	/* Calculate the pid control value by location formula */
	/* u(k) = e(k)*Kp + (e(k)-e(k-1))*Kd + integral*Ki */
	output = error * pid->Kp + (error - pid->previous_err1) * pid->Kd
			+ pid->integral_err * pid->Ki;

	/* If the output is out of the range, it will be limited */
	output = MIN(output, pid->max_output);
	output = MAX(output, pid->min_output);

	/* Update previous error */
	pid->previous_err1 = error;

	return output;
}

static float pid_calc_incremental(pid_ctrl_block_t *pid, float error) {
	float output = 0;

	/* Calculate the pid control value by increment formula */
	/* du(k) = (e(k)-e(k-1))*Kp + (e(k)-2*e(k-1)+e(k-2))*Kd + e(k)*Ki */
	/* u(k) = du(k) + u(k-1) */
	output = (error - pid->previous_err1) * pid->Kp
			+ (error - 2 * pid->previous_err1 + pid->previous_err2) * pid->Kd
			+ error * pid->Ki + pid->last_output;

	/* If the output is beyond the range, it will be limited */
	output = MIN(output, pid->max_output);
	output = MAX(output, pid->min_output);

	/* Update previous error */
	pid->previous_err2 = pid->previous_err1;
	pid->previous_err1 = error;

	/* Update last output */
	pid->last_output = output;

	return output;
}

int pid_new_control_block(const pid_ctrl_config_t *config,
		pid_ctrl_block_handle_t *ret_pid) {
	int ret = 0;
	pid_ctrl_block_t *pid = NULL;
	pid = calloc(1, sizeof(pid_ctrl_block_t));
	*ret_pid = pid;
	return ret;
}

int pid_del_control_block(pid_ctrl_block_handle_t pid) {
	if (pid) {
		free(pid);
	}

	return 0;
}

int pid_compute(pid_ctrl_block_handle_t pid, float input_error,
		float *ret_result) {
	*ret_result = pid->calculate_func(pid, input_error);
	return 0;
}

int pid_update_parameters(pid_ctrl_block_handle_t pid,
		const pid_ctrl_parameter_t *params) {
	pid->Kp = params->kp;
	pid->Ki = params->ki;
	pid->Kd = params->kd;
	pid->max_output = params->max_output;
	pid->min_output = params->min_output;
	pid->max_integral = params->max_integral;
	pid->min_integral = params->min_integral;
	/* Set the calculate function according to the PID type */
	switch (params->cal_type) {
	case PID_CAL_TYPE_INCREMENTAL:
		pid->calculate_func = pid_calc_incremental;
		break;
	case PID_CAL_TYPE_POSITIONAL:
		pid->calculate_func = pid_calc_positional;
		break;
	default:
		break;
	}
	return 0;
}
